package edu.wpi.first.wpilibj.swarm;

public class Angle {
	double ang;
	
	Angle() {
		ang = 0.0;
	}
	
	// WTH? Java's "modulo" isn't actually a modulo... fmod in C too!!!
	double modulo(double a, double b) {
		if (a >= 0.0)
			return a % b;
		else
			return (a % b) + b;
	}
	
	void setAngle(double angle) {
		ang = modulo((angle + Math.PI), (2*Math.PI)) - Math.PI;
	}
	
	void addAngle(double addend) {
		ang = modulo((ang + addend + Math.PI), (2*Math.PI)) - Math.PI;
	}
	
	double getDiff(Angle tgt) {
		return modulo((tgt.getAngle() - ang + Math.PI), (2*Math.PI)) - Math.PI;
	}
	
	double getAngle() {
		return ang;
	}

	void rotate(Vector in, Vector out) {
		out.setX(Math.cos(ang) * in.getX() - Math.sin(ang) * in.getY());
		out.setY(Math.sin(ang) * in.getX() + Math.cos(ang) * in.getY());
	}
}